import rclpy
from rclpy.node import Node
from rclpy.parameter import Parameter

class ParameterManipulator(Node):
    def __init__(self):
        super().__init__('parameter_manipulator')

        # 声明参数，这里我们声明 `calculated_obstacles` 参数并设置默认值
        self.declare_parameter("calculated_obstacles", [0.0, 0.0, 0.0])
        
        # 获取参数
        self.get_param()

        # 修改参数
        #self.modify_param()

        # 设置修改后的参数
        #self.set_param()

    def get_param(self):
        # 获取 `calculated_obstacles` 参数
        param_name = "calculated_obstacles"
        try:
            param = self.get_parameter(param_name)
            if param.type_ == Parameter.Type.DOUBLE_ARRAY:
                self.get_logger().info(f"Current value of {param_name}: {param.value}")
            else:
                self.get_logger().warn(f"{param_name} is not a DOUBLE_ARRAY type.")
        except rclpy.exceptions.ParameterNotDeclaredException:
            self.get_logger().warn(f"{param_name} has not been declared yet.")

    def modify_param(self):
        # 修改参数的值，这里假设我们替换数组中的内容
        new_values = [1.23, 4.56, 7.89]  # 新的 double 数组
        self.get_logger().info(f"Setting new values for 'calculated_obstacles': {new_values}")

        # 更新参数值
        self.declare_parameter("calculated_obstacles", new_values)

    def set_param(self):
        # 设置修改后的参数
        param_name = "calculated_obstacles"
        new_param = Parameter(param_name, Parameter.Type.DOUBLE_ARRAY, [1.23, 4.56, 7.89])
        self.set_parameters([new_param])  # 设置参数到 ROS 2 参数服务器
        self.get_logger().info(f"Updated parameter {param_name} to: {new_param.value}")


def main(args=None):
    rclpy.init(args=args)

    try:
        node = ParameterManipulator()

        # 让节点运行一段时间，执行任务
        rclpy.spin_once(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(f"Error occurred: {e}")
    finally:
        if 'node' in locals():
            node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
